// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
//     backends/cpp/templates/function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------

#pragma once

#include <matrix/math.hpp>

namespace sym {

/**
 * This function was autogenerated from a symbolic function. Do not modify by hand.
 *
 * Symbolic function: terr_est_compute_flow_xy_innov_var_and_hx
 *
 * Args:
 *     terrain_vpos: Scalar
 *     P: Scalar
 *     q_att: Matrix41
 *     v: Matrix31
 *     pos_z: Scalar
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Matrix21
 *     H: Scalar
 */
template <typename Scalar>
void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P,
                                       const matrix::Matrix<Scalar, 4, 1>& q_att,
                                       const matrix::Matrix<Scalar, 3, 1>& v, const Scalar pos_z,
                                       const Scalar R, const Scalar epsilon,
                                       matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
                                       Scalar* const H = nullptr) {
  // Total ops: 28

  // Input arrays

  // Intermediate terms (4)
  const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) -
                       std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2));
  const Scalar _tmp1 = pos_z - terrain_vpos;
  const Scalar _tmp2 =
      -_tmp1 + epsilon * (2 * math::min<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);
  const Scalar _tmp3 = P * std::pow(_tmp0, Scalar(2)) / std::pow(_tmp2, Scalar(4));

  // Output terms (2)
  if (innov_var != nullptr) {
    matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);

    _innov_var(0, 0) = R + _tmp3 * std::pow(v(1, 0), Scalar(2));
    _innov_var(1, 0) = R + _tmp3 * std::pow(v(0, 0), Scalar(2));
  }

  if (H != nullptr) {
    Scalar& _h = (*H);

    _h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
  }
}  // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
}  // namespace sym
